#include "controller/encoder.h"
#include "controller/mc_config.h"
#include "math/fast_math.h"
#include "os/os.h"  // IWYU pragma: export
#include "utils/utils.h"

#if CONFIG_USE_MAG_ENCODER
#define LOG_DEBUG 1
#if (LOG_DEBUG)
#include "utils/logger.h"
#define enc_debug(fmt, args...)   sys_debug("Encoder:"fmt, ##args)
#else
#define enc_debug(fmt, args...)
#endif

static void encoder_init_default(mag_encoder_t *enc);
static void encoder_check_timer_handler(os_timer_t *timer);
static void encoder_abs_angle_handler(void *ctx, float pulse_count);

__STATIC_INLINE float _abi_count(mag_encoder_t *enc) {
#if (CONFIG_ENCODER_CC_INVERT)
	return (enc->cpr - 1 - enc_count());//enc_count [0 - (cpr-1)]
#else
	return enc_count();
#endif
}

__STATIC_INLINE void encoder_pll_update_gain(mag_encoder_t *enc) {
	float omega = rpm_2_rads(_abs(enc->velocity), enc->motor_poles);
	enc->pll_bandwidth_shadow = map_limited(omega, 0, enc->omega_for_max_bandwith, enc->pll_min_bandwith, enc->pll_max_bandwith);	
	if (enc->pll_bandwidth_shadow != enc->pll_bandwidth) {
		enc->pll_bandwidth = enc->pll_bandwidth_shadow;
		pll_set_bandwidth(&enc->pll, enc->pll_bandwidth);
	}
}

static void _init_pll(mag_encoder_t *enc) {
	enc->pll.ts = mc_conf()->ts;
	enc->pll_bandwidth = 0;
	enc->pll_bandwidth_shadow = hz_2_w_rads(300);
	encoder_pll_update_gain(enc);
	pll_reset(&enc->pll, _abi_count(enc));
}


void encoder_init(mag_encoder_t *enc) {
	encoder_init_default(enc);
	enc->init_time = os_timestemp();
	if (!enc->b_inited) {
		enc_hw_intf_init(encoder_abs_angle_handler, enc);
		os_timer_init(&enc->check_timer, encoder_check_timer_handler);
		os_timer_post(&enc->check_timer, 100);
		enc->b_inited = true;
	}
}

void encoder_set_bandwidth(mag_encoder_t *enc, float bandwidth) {
	enc->pll_bandwidth_shadow = bandwidth;
}

static void encoder_init_default(mag_encoder_t *enc) {
	enc->cpr = CONFIG_MAG_ENCOCDER_MAX_RES;
	enc->enc_offset = mc_conf()->m.encoder_offset;
	enc->motor_poles = mc_conf()->m.poles;
	enc->pll_max_bandwith = hz_2_w_rads(300);
	enc->pll_min_bandwith = hz_2_w_rads(50);
	enc->omega_for_max_bandwith = rpm_2_rads(enc->pll_max_bandwith, enc->motor_poles);
	enc->b_pos_sync = false;
	enc->direction = MOT_DIR_POSITIVE;
	enc->abi_angle = 0;
	enc->pwm_angle = 0;
	enc->est_angle_counts = 0;
	enc->velocity = 0;
	enc->velocity_filted = 0;
	enc->position = 0;
	enc->interpolation = 0;
	enc->enc_maybe_err = ENCODER_NO_ERR;
	_init_pll(enc);
}

__STATIC_INLINE bool encoder_run_pll(mag_encoder_t *enc, float cnt) {
	// count / max_res * 60 * 32768 / rpm_base
	encoder_pll_update_gain(enc);
	float vel_counts = (float)pll_run(&enc->pll, cnt, (enc->cpr - 1.0f));
	enc->velocity = vel_counts * 60.0f  / (float)enc->cpr;
	enc->est_vel_cnt_filted = lowpass_filter(enc->est_vel_cnt_filted, vel_counts, 0.1f);
	enc->est_angle_counts = enc->pll.observer;
    bool snap_to_zero_vel = false;
	enc->velocity_filted = lowpass_filter(enc->velocity_filted, enc->velocity, 0.1f);
    if (_abs(enc->pll.out) < enc->pll.ts * enc->pll.ki/2) {
        enc->velocity_filted = enc->velocity = enc->pll.out = 0;  // align delta-sigma on zero to prevent jitter
        snap_to_zero_vel = true;
    }
	enc->est_elec_w_filted = rpm_2_rads(enc->velocity_filted, enc->motor_poles);
	return snap_to_zero_vel;
}

float encoder_update_elec_angle(mag_encoder_t *enc) {
	if (!enc->b_pos_sync) {
		return enc->pwm_angle;
	}
	float cnt = _abi_count(enc);
    bool snap_to_zero_vel = encoder_run_pll(enc, cnt);

	float rads_per_count = M_2PI * (float)enc->motor_poles/enc->cpr;

	if (snap_to_zero_vel) {
		enc->interpolation = rads_per_count * 0.5f;
	}else {
		if (cnt == enc->last_cnt) {
			enc->interpolation += angle_from_We_Ts(enc->est_elec_w_filted, mc_conf()->ts);
			enc->interpolation = clamp_f(enc->interpolation, -rads_per_count, rads_per_count);
		}else {
			enc->interpolation = 0;
		}
	}
	float angle_count = cnt;
	float angle = angle_from_pulse_cnt(angle_count) * enc->motor_poles + enc->enc_offset + enc->interpolation;
	norm_angle_rad(angle);
	enc->abi_angle = angle;
	float delta_cnt = cnt - enc->last_cnt;
	if (delta_cnt < -enc->cpr/2) {
		delta_cnt = enc->cpr - enc->last_cnt + cnt + 1;
	}else if (delta_cnt > enc->cpr/2) {
		delta_cnt = cnt - enc->cpr - enc->last_cnt - 1;
	}

	enc->position += delta_cnt / enc->cpr * M_2PI;
	enc->last_cnt = cnt;
	enc->last_us = cpu_get_usecond();
	return enc->abi_angle;
}

float encoder_get_elec_angle(mag_encoder_t *enc) {
	return enc->abi_angle;
}

void encoder_produce_error(mag_encoder_t *enc, bool error) {
	//enc->produce_error = error;
}

/* must called with irq disabled */
void encoder_enable_pos_lock(mag_encoder_t *enc, bool en) {
	enc->position = 0;
}

u8 encoder_may_error(mag_encoder_t *enc) {
	return enc->enc_maybe_err;
}

float encoder_get_velocity(mag_encoder_t *enc) {
	if (enc->enc_maybe_err != ENCODER_NO_ERR) {
		return 0;
	}
	return enc->velocity;
}

float encoder_get_position(mag_encoder_t *enc) {
	return enc->position;
}

bool encoder_have_error(mag_encoder_t *enc) {
#ifdef USE_MEX_CMD
	return false;
#else
	if ((os_timestemp() >= 1000) && (enc->abi_pwm_count < 10 || enc->pwm_errors > 10)) {
		return true;
	}
	return false;
#endif
}

void encoder_abi_irq_handler(mag_encoder_t *enc, u32 count) {

}

static void encoder_sync_abs_to_abi(mag_encoder_t *enc) {
	u32 mask = cpu_enter_critical();
	enc_set_count(enc->pwm_count);
	enc->last_cnt = enc->pwm_count;
	enc->pll.observer = (float)enc->pwm_count;
	enc->abi_angle = enc->pwm_angle;
	enc->b_pos_sync = true;
	pll_reset(&enc->pll, (float)_abi_count(enc));
	cpu_exit_critical(mask);
}

/* 如果初始没有绝对位置定位，需要控制层调用encoder_align_position接口，进行编码器和实际位置的对齐 */
bool encoder_align_position(mag_encoder_t *enc, float pos) {
	u32 pulse_count = pulse_cnt_from_duty(pos/M_2PI);
	enc->pwm_count = (u32)pulse_count;
	float angle = angle_from_pulse_cnt(pulse_count) * enc->motor_poles + enc->enc_offset;	
	norm_angle_rad(angle);
	enc->pwm_angle = angle;
	encoder_sync_abs_to_abi(enc);
	return true;
}

bool encoder_is_ready(mag_encoder_t *enc) {
	return enc->b_pos_sync;
}


/*PWM 信号捕获一个周期的处理 */
static void encoder_abs_angle_handler(void *ctx, float pulse_count) {
	mag_encoder_t *enc = (mag_encoder_t *)ctx;
	if (os_delta_ms(enc->init_time) < 50) {
		return;
	}
	enc->pwm_count = pulse_count;
	float angle = angle_from_pulse_cnt(pulse_count) * enc->motor_poles + enc->enc_offset;	
	norm_angle_rad(angle);
	enc->pwm_angle = angle;
	if (!enc->b_pos_sync && enc->abi_pwm_count++ >= 10) {
		encoder_sync_abs_to_abi(enc);
	}
	enc->pwm_errors = 0;
	os_timer_post(&enc->check_timer, 100);
}


static void encoder_check_timer_handler(os_timer_t *timer) {
#ifndef USE_MEX_CMD
	mag_encoder_t *enc = container_of(timer, mag_encoder_t, check_timer);
	enc->pwm_errors ++;
	enc->abi_pwm_count = 0;
	os_timer_post(timer, 100);
#endif
}

float encoder_get_abs_angle(mag_encoder_t *enc) {
#if (CONFIG_ENCODER_CC_INVERT)
	float angle = M_2PI - (enc->pwm_angle - enc->enc_offset) + enc->enc_offset;
	norm_angle_rad(angle);
	enc->pwm_angle = angle;
#endif
	return enc->pwm_angle;
}

float encoder_get_abi_angle(mag_encoder_t *enc) {
	float cnt = _abi_count(enc);
	float angle = angle_from_pulse_cnt(cnt) * enc->motor_poles + enc->enc_offset;
	norm_angle_rad(angle);
	return (float)angle;
}

void encoder_log(mag_encoder_t *enc) {
	enc_debug("angle %f, %f, %f, %f\n", rads_2_degree(enc->abi_angle), rads_2_degree(mc_conf()->m.encoder_offset), rads_2_degree(encoder_get_abi_angle(enc)), rads_2_degree(encoder_get_abs_angle(enc)));
	enc_debug("PWM sync: %d, err=%d; %f, %f\n", enc->b_pos_sync, enc->pwm_errors, _abi_count(enc), enc->pwm_count);
}

#endif

